#ifndef _DATABASE_H_
#define _DATABASE_H_
#include "ros/ros.h"
#include "rads_base.hpp"
#include "std_msgs/String.h"
#include <sstream> // 用于字符串流
#include <vector>  // 用于整数数组
#include <iostream>
#include <sqlite3.h>
#include <ctime>
#include <string>
#include <iomanip>
#include <cstdlib>
#include <sys/wait.h>
#include "ros/time.h" // 用于ros::Time
#include <tf/transform_listener.h>
#include <thread>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <fstream>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "rads/rads_scmdret.h"
#include "rads/rads_cmd.h"
#include "rads/rads_scmd.h"
#include "rads/rads_acks.h"
#include "rads/rads_version.h"
#include "rads/rads_versions.h"
#include "rads/rads_driving.h"
#include "rads/rads_trackext.h"
#include <chrono>
#include <mutex>
#include <cmath>
#include <stdexcept>
#include <functional>
#include <std_msgs/Float32.h>
#include <rads/TaskMessage.h>
#include <unordered_map>
#include <queue> // 因为使用了 std::queue
using namespace std;

extern std::mutex g_db_mutex;
/**定义数据结构体start */
typedef struct
{
    double x;
    double y;
    double z;
    double lidar_heading;
    double ndt_matching;
    double rtk_x;
    double rtk_y;
    double rtk_z;
    double rtk_heading;
    double lng;
    double lat;
    int position_flag;
    int orientation_flag;
    int num_point;
    int stop_flag;
    int stop_time;
    int line_id;
    int map_id;
    int pid;
    int ext;
    int stopid;
    string describe;
} car;
typedef struct
{
    int model;
    int steering;
    int acc;
    int brake;
    int gear;
    int target_speed;
    int rpml;
    int rpmr;
    int lignt;
    int audio;
    int sign;
    int camera_hor;
    int camera_ver;
    int unknow_1;
    int unknow_2;
    int unknow_3;
    int unknow_4;
    int unknow_5;
    int map_id;
    int line_id;
    int num_point;
    int alltotal;
    string ctime;
} car_driving_log;

typedef struct
{
    double speed;
    double x;
    double y;
    double z;
    double lidar_heading;
    double ndt_matching;
    double rtk_x;
    double rtk_y;
    double rtk_z;
    double rtk_heading;
    double lng;
    double lat;
    int position_flag;
    int orientation_flag;
    string ctime;
} car_log;

typedef struct
{

    double x;
    double y;
    double z;
    double ox;
    double oy;
    double oz;
    double ow;
    int map_id;
    double xx;
    double yy;
    double head;
} fist_pose;

typedef struct
{
    int num_point;
    double x;
    double y;
    double z;
    double lidar_heading;
    double rtk_x;
    double rtk_y;
    double rtk_z;
    double rtk_heading;
    double lng;
    double lat;
    int position_flag;
    int orientation_flag;
} go_back;

typedef struct
{
    int num;
    int map_id;
    double x;
    double y;
    double z;
    double lidar_heading;
    double ndt_matching;
    double rtk_x;
    double rtk_y;
    double rtk_z;
    double lng;
    double lat;
    double rtk_heading;
    int position_flag;
    int orientation_flag;
} point_all;

typedef struct
{
    double x;
    double y;
    double z;
    double lidar_heading;
    double ndt_matching;
    double rtk_x;
    double rtk_y;
    double rtk_z;
    double lng;
    double lat;
    double rtk_heading;
    int position_flag;
    int orientation_flag;
    int num_point;
    int alltotal;
} point_data;

typedef struct
{
    int num;
    int map_id;
    double x;
    double y;
    double z;
    double ox;
    double oy;
    double oz;
    double ow;
    double lidar_heading;
    double ndt_matching;
    double rtk_x;
    double rtk_y;
    double rtk_z;
    double lng;
    double lat;
    double rtk_heading;
    int position_flag;
    int orientation_flag;
} start_machine;

typedef struct
{
    int type;
    string cmd;
    string ctime;
} tcp_cmd;

typedef struct
{
    int type;
    string publish_date;
    string publish_type;
    string package_size;
    string is_must;
    string node_name;
    string process_name;
    string major_minor_patch;
    string md5_code;
    string temp_down_path;
    string description;
    string ext;
    string ext2;
} version;

typedef struct
{
    int model;
    int soc;
    int err;
    int rpm_l;
    int rpm_r;
    int key_1;
    int key_2;
    int key_3;
    int key_4;
    int key_5;
    int key_6;
    int key_7;
    int key_8;
    int ladar_1;
    int ladar_2;
    int ladar_3;
    int ladar_4;
    int ladar_5;
    int ladar_6;
    int ladar_7;
    int ladar_8;
    int unknow_1;
    int unknow_2;
    int unknow_3;
    int unknow_4;
    int unknow_5;
    int unknow_6;
    int heartbeat;
    string ctime;
} pcb_log;

typedef struct
{
    int ts;
    int uid;
    int opt;
    string vehicleCode;
    int executeType;
    string cron;
    string taskCode;
    std::vector<uint32_t> num; // 对应ROS消息中的uint32[]
    string data;
    std::vector<std::string> dataItems; // 解析后的data数组
    string addtime;
    int turn;
    string uptime;
} car_cron;
typedef struct
{
    int carcron_id;
    int num;
    string data;
} car_cron_post;
/**定义数据结构体end */
static car_log latest_car_log_data;
static pcb_log latest_pcb_log_data;
static bool car_log_updated = false;
static bool pcb_log_updated = false;
/**任意点导航 start*/
struct ListNode
{
    int value;
    ListNode *next;
    ListNode(int x) : value(x), next(nullptr) {}
};
struct AxisData
{
    double x;
    double y;
    AxisData() {};
    AxisData(double x, double y) : x(x), y(y) {}
};
ListNode *lists[180];
vector<AxisData> lidar_points;
vector<double> Lidar_points_x;
vector<double> Lidar_points_y;
vector<double> Nav_x;
vector<double> Nav_y;
int Path_test[1000];
int Current_NUM;
/**任意点导航 end*/

string dbName = DBNAME;
/**数据库连接库start*/
class SQLiteWrapper
{
public:
    SQLiteWrapper(const std::string &dbName);
    ~SQLiteWrapper();
    bool update(const std::string &sql);
    bool insert(const std::string &sql);
    bool deleteRow(const std::string &sql);
    std::vector<std::vector<std::string>> select(const std::string &sql);
    sqlite3 *db() { return db_; }         // 添加获取db指针的方法
    int getLastInsertRowid();             // 获取最后插入的行ID
    bool execute(const std::string &sql); // 执行SQL语句

private:
    sqlite3 *db_;
    void checkError(int rc, const char *msg = nullptr);
    std::vector<std::string> getColumnNames(sqlite3_stmt *stmt);
    std::vector<std::vector<std::string>> getRows(sqlite3_stmt *stmt);
};
// 执行SQL语句
bool SQLiteWrapper::execute(const std::string &sql)
{
    if (!db_)
    {
        throw std::runtime_error("Database connection is not open");
    }

    char *errMsg = nullptr;
    int rc = sqlite3_exec(db_, sql.c_str(), nullptr, nullptr, &errMsg);

    if (rc != SQLITE_OK)
    {
        std::cerr << "SQL execute error: " << errMsg << std::endl;
        sqlite3_free(errMsg);
        return false;
    }

    return true;
}
// 获取最后插入的行ID
int SQLiteWrapper::getLastInsertRowid()
{
    if (!db_)
    {
        throw std::runtime_error("Database connection is not open");
    }

    // 使用SQLite内置函数last_insert_rowid()
    sqlite3_stmt *stmt;
    const char *sql = "SELECT last_insert_rowid();";

    int rc = sqlite3_prepare_v2(db_, sql, -1, &stmt, nullptr);
    if (rc != SQLITE_OK)
    {
        checkError(rc, "Failed to prepare last_insert_rowid statement");
        return -1;
    }

    int rowid = -1;
    if (sqlite3_step(stmt) == SQLITE_ROW)
    {
        rowid = sqlite3_column_int(stmt, 0);
    }

    sqlite3_finalize(stmt);
    return rowid;
}
SQLiteWrapper::SQLiteWrapper(const std::string &dbName) : db_(nullptr)
{
    int rc = sqlite3_open(dbName.c_str(), &db_);
    if (rc)
    {
        throw std::runtime_error("Can't open database: " + std::string(sqlite3_errmsg(db_)));
    }
    // 启用WAL模式和设置超时
    sqlite3_exec(db_, "PRAGMA journal_mode=WAL; PRAGMA synchronous=NORMAL;", nullptr, nullptr, nullptr);
    sqlite3_busy_timeout(db_, 5000); // 5秒超时
}
SQLiteWrapper::~SQLiteWrapper()
{
    if (db_)
    {
        sqlite3_close(db_);
    }
}
void SQLiteWrapper::checkError(int rc, const char *msg)
{
    if (rc != SQLITE_OK)
    {
        if (msg)
        {
            throw std::runtime_error(std::string(msg) + ": " + std::string(sqlite3_errmsg(db_)));
        }
        else
        {
            throw std::runtime_error(std::string(sqlite3_errmsg(db_)));
        }
    }
}
bool SQLiteWrapper::update(const std::string &sql)
{
    char *errMsg = nullptr;
    int rc = sqlite3_exec(db_, sql.c_str(), nullptr, nullptr, &errMsg);
    if (rc != SQLITE_OK)
    {
        std::cerr << "SQL error: " << errMsg << std::endl;
        sqlite3_free(errMsg);
        return false;
    }
    return true;
}
bool SQLiteWrapper::insert(const std::string &sql)
{
    // 对于insert操作，我们可以重用update方法，因为它们在SQLite中通常没有返回结果集
    return update(sql);
}
bool SQLiteWrapper::deleteRow(const std::string &sql)
{
    char *errMsg = nullptr;
    int rc = sqlite3_exec(db_, sql.c_str(), nullptr, nullptr, &errMsg);
    if (rc != SQLITE_OK)
    {
        std::cerr << "SQL error: " << errMsg << std::endl;
        sqlite3_free(errMsg);
        return false;
    }
    // 如果需要，可以返回受影响的行数（但 sqlite3_exec 不直接提供这个信息）
    // 可以通过 sqlite3_changes() 获取，但通常不需要在这里立即返回
    return true;
}
std::vector<std::string> SQLiteWrapper::getColumnNames(sqlite3_stmt *stmt)
{
    std::vector<std::string> columnNames;
    int columnCount = sqlite3_column_count(stmt);
    for (int i = 0; i < columnCount; ++i)
    {
        const char *columnName = sqlite3_column_name(stmt, i);
        columnNames.push_back(std::string(columnName));
    }
    return columnNames;
}
std::vector<std::vector<std::string>> SQLiteWrapper::getRows(sqlite3_stmt *stmt)
{
    std::vector<std::vector<std::string>> rows;
    while (sqlite3_step(stmt) == SQLITE_ROW)
    {
        std::vector<std::string> row;
        int columnCount = sqlite3_column_count(stmt);
        for (int i = 0; i < columnCount; ++i)
        {
            const unsigned char *text = sqlite3_column_text(stmt, i);
            row.push_back(text ? reinterpret_cast<const char *>(text) : "");
        }
        rows.push_back(row);
    }
    return rows;
}
std::vector<std::vector<std::string>> SQLiteWrapper::select(const std::string &sql)
{
    sqlite3_stmt *stmt;
    const char *tail;
    int rc = sqlite3_prepare_v2(db_, sql.c_str(), -1, &stmt, &tail);
    if (rc != SQLITE_OK)
    {
        checkError(rc, "Failed to prepare statement");
        // 假设 checkError 在遇到错误时会抛出异常或处理资源清理
        sqlite3_finalize(stmt); // 如果 checkError 不负责，这里需要确保释放资源
        return {};              // 或者其他错误处理
    }
    std::vector<std::vector<std::string>> result;
    auto rows = getRows(stmt);
    result.insert(result.end(), rows.begin(), rows.end());

    sqlite3_finalize(stmt);
    return result;
}
/**数据库连接库end */

// 获取时间 年/月/日：小时/分钟/秒
std::string getCurrentTimeString()
{
    // 获取当前时间点
    auto now = std::chrono::system_clock::now();
    // 转换为 time_t 类型（C 风格时间）
    std::time_t now_time_t = std::chrono::system_clock::to_time_t(now);
    // 转换为 tm 结构（本地时间）
    std::tm *now_tm = std::localtime(&now_time_t);
    // 使用 ostringstream 来格式化时间字符串
    std::ostringstream oss;
    oss << std::put_time(now_tm, "%Y-%m-%d %H:%M:%S");
    return oss.str();
}
// 处理sql时间格式
std::string escapeString(const std::string &input)
{
    std::string output;
    output.reserve(input.size() + 2); // 预留空间，包括可能的转义字符和引号
    // 添加开头的单引号
    output += '\'';
    for (char c : input)
    {
        if (c == '\'')
        {
            // 转义单引号
            output += "''";
        }
        else
        {
            output += c;
        }
    }
    // 添加结尾的单引号
    output += '\'';
    return output;
}

// 辅助函数：检查字符串是否以特定前缀开头
bool startsWith(const std::string &str, const std::string &prefix)
{
    return str.size() >= prefix.size() &&
           str.compare(0, prefix.size(), prefix) == 0;
}

void parseDataString(const std::string &dataStr, std::vector<std::string> &result)
{
    result.clear();

    // 移除首尾的方括号和可能的空白字符
    std::string content = dataStr;
    content.erase(std::remove_if(content.begin(), content.end(), ::isspace), content.end());

    if (content.empty())
        return;

    // 确保以[开头和]结尾
    if (content.front() != '[' || content.back() != ']')
    {
        // 如果不是JSON数组格式，直接返回整个字符串
        result.push_back(content);
        return;
    }

    // 移除方括号
    content = content.substr(1, content.size() - 2);

    bool inQuote = false;
    size_t start = 0;
    std::vector<std::string> items;

    // 解析JSON数组中的每个元素
    for (size_t i = 0; i < content.size(); ++i)
    {
        if (content[i] == '"')
        {
            inQuote = !inQuote; // 切换引号状态
        }
        else if (content[i] == ',' && !inQuote)
        {
            // 只有在引号外的逗号才是分隔符
            std::string item = content.substr(start, i - start);
            // 移除首尾引号
            if (item.size() >= 2 && item.front() == '"' && item.back() == '"')
            {
                item = item.substr(1, item.size() - 2);
            }
            items.push_back(item);
            start = i + 1;
        }
    }

    // 处理最后一个元素
    if (start < content.size())
    {
        std::string item = content.substr(start);
        // 移除首尾引号
        if (item.size() >= 2 && item.front() == '"' && item.back() == '"')
        {
            item = item.substr(1, item.size() - 2);
        }
        items.push_back(item);
    }

    // 合并特定模式的项
    for (size_t i = 0; i < items.size(); ++i)
    {
        std::string current = items[i];

        // 检查是否满足合并条件: 当前项以##1500001,29,开头，且下一项存在并以##1500001,291,开头
        if (i + 1 < items.size() &&
            startsWith(current, "##1500016,29,") &&
            startsWith(items[i + 1], "##1500016,291,"))
        {
            // 合并两项
            current += "," + items[i + 1];
            i++; // 跳过下一项，因为已经合并
        }

        result.push_back(current);
    }
}

// 四元数转航向角
double quaternionToEulerAngle(double ox, double oy, double oz, double ow)
{
    // 创建tf四元数
    tf::Quaternion quat(ox, oy, oz, ow);
    // 创建旋转矩阵
    tf::Matrix3x3 mat(quat); // 注意使用tf命名空间
    double roll, pitch, yaw;
    mat.getRPY(roll, pitch, yaw);
    // 转换为度数并调整范围
    double angle_deg = yaw * (180.0 / M_PI); // 弧度转度
    angle_deg = -angle_deg + 180.0;          // 调整方向
    if (angle_deg < 0)
        angle_deg += 360.0; // 确保在 [0, 360) 范围内

    return angle_deg;
}

// 接受网络传来指令存库
bool insert_cmd(const tcp_cmd &cmd)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO tcp_cmd (type,cmd,ctime) VALUES(" +
                            to_string(cmd.type) + ", " +
                            escapeString(cmd.cmd) + ", " +
                            escapeString(cmd.ctime) + ");";
    return db.insert(insertSql);
}

bool insert_fist_pose(const fist_pose &fist_pose_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO fist_pose (map_id) VALUES(" +
                            to_string(fist_pose_data.map_id) + ");";
    return db.insert(insertSql);
}

// 插入车辆日志
bool insert_car_log(const car_log &car_log_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO car_log (speed,x,y,z,lidar_heading,ndt_matching,rtk_x,rtk_y,rkt_z,lng,lat,rtk_heading,position_flag,orientation_flag,ctime) VALUES(" +
                            to_string(car_log_data.speed) + ", " +
                            to_string(car_log_data.x) + ", " +
                            to_string(car_log_data.y) + ", " +
                            to_string(car_log_data.z) + ", " +
                            to_string(car_log_data.lidar_heading) + ", " +
                            to_string(car_log_data.ndt_matching) + ", " +
                            to_string(car_log_data.rtk_x) + ", " +
                            to_string(car_log_data.rtk_y) + ", " +
                            to_string(car_log_data.rtk_z) + ", " +
                            to_string(car_log_data.lng) + ", " +
                            to_string(car_log_data.lat) + ", " +
                            to_string(car_log_data.rtk_heading) + ", " +
                            to_string(car_log_data.position_flag) + ", " +
                            to_string(car_log_data.orientation_flag) + ", " +
                            escapeString(car_log_data.ctime) + ");";
    // std::cout << insertSql << std::endl;
    return db.insert(insertSql);
}
// 插入PCB日志
bool insert_pcb_log(const pcb_log &pcb_log_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO pcb_log (model,soc,err,rpm_l,rpm_r,key_1,key_2,key_3,key_4,key_5,key_6,key_7,key_8,ladar_1,ladar_2,ladar_3,ladar_4,ladar_5,ladar_6,ladar_7,ladar_8,unknow_1,unknow_2,unknow_3,unknow_4,unknow_5,unknow_6,heartbeat,ctime) VALUES(" +
                            to_string(pcb_log_data.model) + ", " +
                            to_string(pcb_log_data.soc) + ", " +
                            to_string(pcb_log_data.err) + ", " +
                            to_string(pcb_log_data.rpm_l) + ", " +
                            to_string(pcb_log_data.rpm_r) + ", " +
                            to_string(pcb_log_data.key_1) + ", " +
                            to_string(pcb_log_data.key_2) + ", " +
                            to_string(pcb_log_data.key_3) + ", " +
                            to_string(pcb_log_data.key_4) + ", " +
                            to_string(pcb_log_data.key_5) + ", " +
                            to_string(pcb_log_data.key_6) + ", " +
                            to_string(pcb_log_data.key_7) + ", " +
                            to_string(pcb_log_data.key_8) + ", " +
                            to_string(pcb_log_data.ladar_1) + ", " +
                            to_string(pcb_log_data.ladar_2) + ", " +
                            to_string(pcb_log_data.ladar_3) + ", " +
                            to_string(pcb_log_data.ladar_4) + ", " +
                            to_string(pcb_log_data.ladar_5) + ", " +
                            to_string(pcb_log_data.ladar_6) + ", " +
                            to_string(pcb_log_data.ladar_7) + ", " +
                            to_string(pcb_log_data.ladar_8) + ", " +
                            to_string(pcb_log_data.unknow_1) + ", " +
                            to_string(pcb_log_data.unknow_2) + ", " +
                            to_string(pcb_log_data.unknow_3) + ", " +
                            to_string(pcb_log_data.unknow_4) + ", " +
                            to_string(pcb_log_data.unknow_5) + ", " +
                            to_string(pcb_log_data.unknow_6) + ", " +
                            to_string(pcb_log_data.heartbeat) + ", " +
                            escapeString(pcb_log_data.ctime) + ");";
    return db.insert(insertSql);
}
// 插入控制车日志
bool insert_driving_log(const car_driving_log &car_driving_log_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO driving_log (model, steering, acc, brake, gear, target_speed,rpml, rpmr, lignt, audio, sign,camera_hor, camera_ver, unknow_1, unknow_2,unknow_3, unknow_4, unknow_5, map_id, line_id,num_point, alltotal, ctime) VALUES(" +
                            to_string(car_driving_log_data.model) + ", " +
                            to_string(car_driving_log_data.steering) + ", " +
                            to_string(car_driving_log_data.acc) + ", " +
                            to_string(car_driving_log_data.brake) + ", " +
                            to_string(car_driving_log_data.gear) + ", " +
                            to_string(car_driving_log_data.target_speed) + ", " +
                            to_string(car_driving_log_data.rpml) + ", " +
                            to_string(car_driving_log_data.rpmr) + ", " +
                            to_string(car_driving_log_data.lignt) + ", " +
                            to_string(car_driving_log_data.audio) + ", " +
                            to_string(car_driving_log_data.sign) + ", " +
                            to_string(car_driving_log_data.camera_hor) + ", " +
                            to_string(car_driving_log_data.camera_ver) + ", " +
                            to_string(car_driving_log_data.unknow_1) + ", " +
                            to_string(car_driving_log_data.unknow_2) + ", " +
                            to_string(car_driving_log_data.unknow_3) + ", " +
                            to_string(car_driving_log_data.unknow_4) + ", " +
                            to_string(car_driving_log_data.unknow_5) + ", " +
                            to_string(car_driving_log_data.map_id) + ", " +
                            to_string(car_driving_log_data.line_id) + ", " +
                            to_string(car_driving_log_data.num_point) + ", " +
                            to_string(car_driving_log_data.alltotal) + ", " +
                            escapeString(car_driving_log_data.ctime) + ");";
    return db.insert(insertSql);
}
// 插入开机点
bool insert_start_machine(const start_machine &start_machine_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO start_machine (num, map_id, x, y, z, ox, oy, oz, ow,lidar_heading, ndt_matching, rtk_x, rtk_y, rtk_z, lng, lat,rtk_heading, position_flag, orientation_flag) VALUES(" +
                            to_string(start_machine_data.num) + ", " +
                            to_string(start_machine_data.map_id) + ", " +
                            to_string(start_machine_data.x) + ", " +
                            to_string(start_machine_data.y) + ", " +
                            to_string(start_machine_data.z) + ", " +
                            to_string(start_machine_data.ox) + ", " +
                            to_string(start_machine_data.oy) + ", " +
                            to_string(start_machine_data.oz) + ", " +
                            to_string(start_machine_data.ow) + ", " +
                            to_string(start_machine_data.lidar_heading) + ", " +
                            to_string(start_machine_data.ndt_matching) + ", " +
                            to_string(start_machine_data.rtk_x) + ", " +
                            to_string(start_machine_data.rtk_y) + ", " +
                            to_string(start_machine_data.rtk_z) + ", " +
                            to_string(start_machine_data.lng) + ", " +
                            to_string(start_machine_data.lat) + ", " +
                            to_string(start_machine_data.rtk_heading) + ", " +
                            to_string(start_machine_data.position_flag) + ", " +
                            to_string(start_machine_data.orientation_flag) + ");";
    return db.insert(insertSql);
}
// 删除开机点
bool delete_start_machine(const start_machine &start_machine_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string deleteSql = "DELETE FROM start_machine WHERE map_id = " + std::to_string(start_machine_data.map_id) + " AND num=" + std::to_string(start_machine_data.num) + ";";
    return db.deleteRow(deleteSql);
}
// 创建带图带线路线点
bool insert_car(const car &car_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO car (x, y, z, lidar_heading, ndt_matching,ext,rtk_x, rtk_y, rtk_z, rtk_heading, lng, lat,position_flag, orientation_flag, num_point, stop_flag, stop_time, line_id, map_id) VALUES(" +
                            to_string(car_data.x) + ", " +
                            to_string(car_data.y) + ", " +
                            to_string(car_data.z) + ", " +
                            to_string(car_data.lidar_heading) + ", " +
                            to_string(car_data.ndt_matching) + ", " +
                            to_string(car_data.ext) + ", " +
                            to_string(car_data.rtk_x) + ", " +
                            to_string(car_data.rtk_y) + ", " +
                            to_string(car_data.rtk_z) + ", " +
                            to_string(car_data.rtk_heading) + ", " +
                            to_string(car_data.lng) + ", " +
                            to_string(car_data.lat) + ", " +
                            to_string(car_data.position_flag) + ", " +
                            to_string(car_data.orientation_flag) + ", " +
                            to_string(car_data.num_point) + ", " +
                            to_string(car_data.stop_flag) + ", " +
                            to_string(car_data.stop_time) + ", " +
                            to_string(car_data.line_id) + ", " +
                            to_string(car_data.map_id) + ");";
    return db.insert(insertSql);
}
// 创建带图带线路线点B
bool insert_carB(const car &car_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO carB (x, y, z, lidar_heading, ndt_matching,ext,rtk_x, rtk_y, rtk_z, rtk_heading, lng, lat,position_flag, orientation_flag, num_point, stop_flag, stop_time, line_id, map_id) VALUES(" +
                            to_string(car_data.x) + ", " +
                            to_string(car_data.y) + ", " +
                            to_string(car_data.z) + ", " +
                            to_string(car_data.lidar_heading) + ", " +
                            to_string(car_data.ndt_matching) + ", " +
                            to_string(car_data.ext) + ", " +
                            to_string(car_data.rtk_x) + ", " +
                            to_string(car_data.rtk_y) + ", " +
                            to_string(car_data.rtk_z) + ", " +
                            to_string(car_data.rtk_heading) + ", " +
                            to_string(car_data.lng) + ", " +
                            to_string(car_data.lat) + ", " +
                            to_string(car_data.position_flag) + ", " +
                            to_string(car_data.orientation_flag) + ", " +
                            to_string(car_data.num_point) + ", " +
                            to_string(car_data.stop_flag) + ", " +
                            to_string(car_data.stop_time) + ", " +
                            to_string(car_data.line_id) + ", " +
                            to_string(car_data.map_id) + ");";
    return db.insert(insertSql);
}
// 马踏飞燕
bool insert_point_all(const point_all &point_all_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO point_all (map_id,x,escapeString y, z, lidar_heading, ndt_matching,rtk_x, rtk_y, rtk_z, lng, lat, rtk_heading,position_flag, orientation_flag) VALUES(" +
                            to_string(point_all_data.map_id) + ", " +
                            to_string(point_all_data.x) + ", " +
                            to_string(point_all_data.y) + ", " +
                            to_string(point_all_data.z) + ", " +
                            to_string(point_all_data.lidar_heading) + ", " +
                            to_string(point_all_data.ndt_matching) + ", " +
                            to_string(point_all_data.rtk_x) + ", " +
                            to_string(point_all_data.rtk_y) + ", " +
                            to_string(point_all_data.rtk_z) + ", " +
                            to_string(point_all_data.lng) + ", " +
                            to_string(point_all_data.lat) + ", " +
                            to_string(point_all_data.rtk_heading) + ", " +
                            to_string(point_all_data.position_flag) + ", " +
                            to_string(point_all_data.orientation_flag) + ");";
    return db.insert(insertSql);
}

// 任意点存库
bool insert_point_data(const point_data &point_data_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO point_data (x, y, z, lidar_heading, ndt_matching,rtk_x, rtk_y, rtk_z, lng, lat, rtk_heading,position_flag, orientation_flag,num_point,alltotal) VALUES(" +
                            to_string(point_data_data.x) + ", " +
                            to_string(point_data_data.y) + ", " +
                            to_string(point_data_data.z) + ", " +
                            to_string(point_data_data.lidar_heading) + ", " +
                            to_string(point_data_data.ndt_matching) + ", " +
                            to_string(point_data_data.rtk_x) + ", " +
                            to_string(point_data_data.rtk_y) + ", " +
                            to_string(point_data_data.rtk_z) + ", " +
                            to_string(point_data_data.lng) + ", " +
                            to_string(point_data_data.lat) + ", " +
                            to_string(point_data_data.rtk_heading) + ", " +
                            to_string(point_data_data.position_flag) + ", " +
                            to_string(point_data_data.orientation_flag) + ", " +
                            to_string(point_data_data.num_point) + ", " +
                            to_string(point_data_data.alltotal) + ");";
    return db.insert(insertSql);
}
// goback踩点
bool insert_go_back(const go_back &go_back_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string insertSql = "INSERT INTO go_back (num_point, x, y, z, lidar_heading,rtk_x, rtk_y, rtk_z, rtk_heading,lng, lat, position_flag, orientation_flag) VALUES(" +
                            to_string(go_back_data.num_point) + ", " +
                            to_string(go_back_data.x) + ", " +
                            to_string(go_back_data.y) + ", " +
                            to_string(go_back_data.z) + ", " +
                            to_string(go_back_data.lidar_heading) + ", " +
                            to_string(go_back_data.rtk_x) + ", " +
                            to_string(go_back_data.rtk_y) + ", " +
                            to_string(go_back_data.rtk_z) + ", " +
                            to_string(go_back_data.rtk_heading) + ", " +
                            to_string(go_back_data.lng) + ", " +
                            to_string(go_back_data.lat) + ", " +
                            to_string(go_back_data.position_flag) + ", " +
                            to_string(go_back_data.orientation_flag) + ");";
    return db.insert(insertSql);
}
bool insert_car_cron_data(const car_cron &car_cron_data)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);

    // 开始事务
    if (!db.execute("BEGIN TRANSACTION"))
    {
        ROS_ERROR("Failed to start database transaction");
        return false;
    }

    try
    {
        // 1. 插入carcron表 - num字段存储dataItems的数量
        std::stringstream ss;
        ss << "INSERT INTO carcron (ts, uid, opt, vehicleCode, executeType, cron, taskCode, addtime, num) VALUES ("
           << car_cron_data.ts << ", "
           << car_cron_data.uid << ", "
           << car_cron_data.opt << ", "
           << escapeString(car_cron_data.vehicleCode) << ", "
           << car_cron_data.executeType << ", "
           << escapeString(car_cron_data.cron) << ", "
           << car_cron_data.taskCode << ", "
           << escapeString(car_cron_data.addtime) << ", "
           << car_cron_data.dataItems.size() << ");";

        std::string insertSql = ss.str();
        std::cout << "pradsmessagecmd:" << insertSql << std::endl;
        if (!db.insert(insertSql))
        {
            throw std::runtime_error("Failed to insert into carcron table");
        }

        // 获取最后插入的ID
        int lastInsertId = db.getLastInsertRowid();
        if (lastInsertId <= 0)
        {
            throw std::runtime_error("Failed to get last insert ID");
        }

        // 2. 插入carcron_post表（多条记录）- num字段从1开始递增
        for (size_t i = 0; i < car_cron_data.dataItems.size(); ++i)
        {
            ss.str("");
            ss << "INSERT INTO carcron_post (carcron_id, num, addtime, data) VALUES ("
               << lastInsertId << ", "
               << (i + 1) << ", " // num是从1开始的序号，不使用原数据中的num
               << escapeString(car_cron_data.addtime) << ", "
               << escapeString(car_cron_data.dataItems[i]) << ");";

            insertSql = ss.str();
            if (!db.insert(insertSql))
            {
                throw std::runtime_error("Failed to insert into carcron_post table");
            }
        }

        // 提交事务
        if (!db.execute("COMMIT"))
        {
            throw std::runtime_error("Failed to commit transaction");
        }

        // 调用shell插入cron计划任务
        char command[512];
        if (car_cron_data.executeType == 1)
        {
            snprintf(command, sizeof(command),
                     "cd /home/argo/catkin_ws/src/rads-master/shell && "
                     "./add_cron_job.sh '%s' '/home/argo/catkin_ws/src/rads-master/shell/publisher_once.sh -c %d'",
                     car_cron_data.cron.c_str(),
                     lastInsertId);
            printf("计划行命令: %s\n", command);
            int result = system(command);
            return true;
        }
        else
        {
            snprintf(command, sizeof(command),
                     "cd /home/argo/catkin_ws/src/rads-master/shell && "
                     "./publisher_once.sh -c %d",
                     lastInsertId);
            printf("即将执行命令: %s\n", command);
            int result = system(command);
            return true;
        }
    }
    catch (const std::exception &e)
    {
        // 回滚事务
        db.execute("ROLLBACK");
        ROS_ERROR("Database operation failed: %s", e.what());
        return false;
    }
}

// 清空表go_back
bool delete_go_back()
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string deleteSql = "DELETE FROM go_back;";
    return db.insert(deleteSql);
}
// 清空表point_data
bool delete_point_data()
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string deleteSql = "DELETE FROM point_data;";
    return db.insert(deleteSql);
}

// 总任务表计数加1
bool carcron_add(int cron_id)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);

    // 1. 先查询当前的uptime值
    std::string selectSql = "SELECT uptime FROM carcron WHERE id = " + std::to_string(cron_id) + ";";
    auto result = db.select(selectSql); // 获取查询结果，二维向量

    // 获取当前时间和当前日期
    std::string currentTime = getCurrentTimeString();
    std::string currentDate = currentTime.substr(0, 10); // 提取"YYYY-MM-DD"部分

    // 2. 确定要执行的更新逻辑
    std::string updateSql;

    if (result.empty())
    {
        // 没有找到对应记录
        return false;
    }

    // 由于查询只选择了uptime一列，所以第一行第一列就是uptime的值
    std::string uptime = result[0][0]; // 修正：使用索引访问，而不是键值

    if (uptime.empty() || uptime == "NULL")
    {
        // uptime为空，turn加1
        updateSql = "UPDATE carcron SET "
                    "turn = 1, "
                    "uptime = '" +
                    currentTime + "' "
                                  "WHERE id = " +
                    std::to_string(cron_id) + ";";
    }
    else
    {
        // 提取uptime中的日期部分
        std::string uptimeDate = uptime.substr(0, 10);

        if (uptimeDate != currentDate)
        {
            // uptime不是当天日期，turn设为1
            updateSql = "UPDATE carcron SET "
                        "turn = 1, "
                        "uptime = '" +
                        currentTime + "' "
                                      "WHERE id = " +
                        std::to_string(cron_id) + ";";
        }
        else
        {
            // uptime是当天日期，不修改turn，只更新时间
            updateSql = "UPDATE carcron SET "
                        "turn = turn + 1, "
                        "uptime = '" +
                        currentTime + "' "
                                      "WHERE id = " +
                        std::to_string(cron_id) + ";";
        }
    }

    // 3. 执行更新操作
    return db.update(updateSql);
}

// 获取cron分任务包
std::vector<car_cron_post> fetchCarcrondata(int cron_id)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    // 查询该路径上的所有点位
    std::string sql =
        "SELECT carcron_id, num, data "
        "FROM carcron_post "
        "WHERE carcron_id = " +
        std::to_string(cron_id) +
        " ORDER BY num";

    auto result = db.select(sql);

    if (result.empty())
    {
        throw std::runtime_error("No data found for cron_id=" + std::to_string(cron_id));
    }

    std::vector<car_cron_post> carcronList;
    for (const auto &row : result)
    {
        car_cron_post data;
        data.carcron_id = std::stoi(row[0]);
        data.num = std::stoi(row[1]);
        data.data = row[2];
        carcronList.push_back(data);
    }

    return carcronList;
}

std::string vectorToString(const std::vector<int> &vec)
{
    std::ostringstream oss;
    for (size_t i = 0; i < vec.size(); ++i)
    {
        if (i != 0)
            oss << ",";
        oss << vec[i];
    }
    return oss.str();
}

std::vector<car> BCronfetchCarDataFromDB(int map_id, const std::string &line_id,
                                         const std::vector<int> &point_ids,
                                         int current_position)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);

    // Generate complete point sequence
    std::vector<int> full_sequence;
    std::vector<int> key_point_indices;

    if (point_ids.empty())
    {
        throw std::runtime_error("Point ID list cannot be empty");
    }

    // Debug log: Output input parameters
    ROS_INFO("BCronfetchCarDataFromDB - map_id: %d, line_id: %s, current_position: %d",
             map_id, line_id.c_str(), current_position);
    ROS_INFO("point_ids size: %zu, first target: %d",
             point_ids.size(), point_ids[0]);

    // 1. From current position to first target point
    int first_target = point_ids[0];

    if (first_target > current_position)
    {
        // Move forward
        ROS_INFO("Moving forward: from %d to %d", current_position, first_target);
        for (int i = current_position + 1; i <= first_target; ++i)
        {
            full_sequence.push_back(i);
        }
    }
    else if (first_target < current_position)
    {
        // Move backward
        ROS_INFO("Moving backward: from %d to %d", current_position, first_target);
        for (int i = current_position - 1; i >= first_target; --i)
        {
            full_sequence.push_back(i);
        }
    }
    else
    {
        // Current position is the same as first target
        ROS_INFO("Current position equals first target: %d", current_position);
        full_sequence.push_back(current_position);
    }

    // Check if first step generated data
    if (full_sequence.empty())
    {
        throw std::runtime_error("No point data generated from current position to first target, current_position: " +
                                 std::to_string(current_position) + ", first_target: " + std::to_string(first_target));
    }

    key_point_indices.push_back(full_sequence.size() - 1); // Mark first key point

    // 2. Process path between subsequent target points
    for (size_t i = 1; i < point_ids.size(); ++i)
    {
        int last_point = full_sequence.back();
        int target = point_ids[i];

        ROS_INFO("Processing target point %zu: from %d to %d", i, last_point, target);

        if (target > last_point)
        {
            // Move forward
            for (int j = last_point + 1; j <= target; ++j)
            {
                full_sequence.push_back(j);
            }
        }
        else if (target < last_point)
        {
            // Move backward
            for (int j = last_point - 1; j >= target; --j)
            {
                full_sequence.push_back(j);
            }
        }
        else
        {
            // Target point is same as previous point
            ROS_INFO("Target point same as previous point: %d", target);
            full_sequence.push_back(target);
        }

        key_point_indices.push_back(full_sequence.size() - 1); // Mark key point
    }

    // Check if sequence is empty to avoid invalid SQL
    if (full_sequence.empty())
    {
        throw std::runtime_error("Generated point sequence is empty, cannot build query. point_ids size: " +
                                 std::to_string(point_ids.size()) + ", current_position: " +
                                 std::to_string(current_position));
    }

    // Output generated point sequence for debugging
    ROS_INFO("Generated point sequence size: %zu", full_sequence.size());
    std::string sequence_str;
    for (size_t i = 0; i < full_sequence.size(); ++i)
    {
        if (i > 0)
            sequence_str += ", ";
        sequence_str += std::to_string(full_sequence[i]);
        // Avoid too long log, output first 20 and last 10 only
        if (i == 20 && full_sequence.size() > 30)
        {
            sequence_str += ", ...";
            break;
        }
    }
    ROS_INFO("Point sequence: %s", sequence_str.c_str());

    // Build SQL query
    std::string sql = "WITH points(id, num) AS (";

    for (size_t i = 0; i < full_sequence.size(); ++i)
    {
        if (i != 0)
        {
            sql += " UNION ALL ";
        }
        sql += "SELECT " + std::to_string(i) + ", " + std::to_string(full_sequence[i]);
    }

    sql += ") "
           "SELECT c.x, c.y, c.z, c.lidar_heading, c.ndt_matching, "
           "c.rtk_x, c.rtk_y, c.rtk_z, c.rtk_heading, "
           "c.lng, c.lat, c.position_flag, c.orientation_flag, "
           "c.num_point, c.stop_flag, c.stop_time, c.line_id, c.map_id, c.ext "
           "FROM carB c "
           "JOIN points p ON c.num_point = p.num "
           "WHERE c.map_id = " +
           std::to_string(map_id) +
           " AND c.line_id = '" + line_id + "' "
                                            "ORDER BY p.id";

    ROS_INFO("Executing SQL: %s", sql.c_str());

    // Subsequent processing remains unchanged...
    try
    {
        // db.execute("COMMIT;");
        auto result = db.select(sql);

        if (result.empty())
        {
            throw std::runtime_error("No data found for map_id=" + std::to_string(map_id) +
                                     ", line_id=" + line_id + ", points count=" + std::to_string(full_sequence.size()));
        }

        std::vector<car> carDataList;
        for (size_t i = 0; i < result.size(); ++i)
        {
            const auto &row = result[i];
            car data;

            try
            {
                data.x = std::stod(row[0]);
                data.y = std::stod(row[1]);
                data.z = std::stod(row[2]);
                data.lidar_heading = std::stod(row[3]);
                data.ndt_matching = std::stod(row[4]);
                data.rtk_x = std::stod(row[5]);
                data.rtk_y = std::stod(row[6]);
                data.rtk_z = std::stod(row[7]);
                data.rtk_heading = std::stod(row[8]);
                data.lng = std::stod(row[9]);
                data.lat = std::stod(row[10]);
                data.position_flag = std::stoi(row[11]);
                data.orientation_flag = std::stoi(row[12]);
                data.num_point = std::stoi(row[13]);
                data.stop_flag = std::stoi(row[14]);
                data.stop_time = std::stoi(row[15]);
                data.line_id = std::stoi(row[16]);
                data.map_id = std::stoi(row[17]);
                data.ext = std::stoi(row[18]);
            }
            catch (const std::exception &e)
            {
                throw std::runtime_error("Data parsing failed: " + std::string(e.what()) +
                                         " at row " + std::to_string(i));
            }

            // auto it = std::find(key_point_indices.begin(), key_point_indices.end(), i);
            // if (it != key_point_indices.end()) {
            //     data.pid = std::distance(key_point_indices.begin(), it) + 1;
            // } else {
            //     data.pid = 0;
            // }

            carDataList.push_back(data);
        }

        return carDataList;
    }
    catch (const std::exception &e)
    {
        throw std::runtime_error("Database query failed: " + std::string(e.what()) +
                                 ", SQL: " + sql);
    }
}

// 数据结果结构体
struct CarDataResult
{
    std::vector<car> primaryData;
    std::vector<car> secondaryData;
};

// 路径段信息结构体
struct PathSegment
{
    std::vector<int> sequence;
    int start;
    int end;
    bool is_forward;
    size_t key_point_index;
    int target_stopid; // 当前段的目标stopid
};

// 辅助函数：判断路线类型
bool isNumericLine(const std::string &line_id)
{
    return !line_id.empty() && std::all_of(line_id.begin(), line_id.end(), ::isdigit);
}

bool isAlphaLine(const std::string &line_id)
{
    return !line_id.empty() && std::all_of(line_id.begin(), line_id.end(), ::isalpha);
}

// 辅助函数：向量转字符串
template <typename T>
std::string vectorToString(const std::vector<T> &vec)
{
    std::stringstream ss;
    for (size_t i = 0; i < vec.size(); ++i)
    {
        if (i > 0)
            ss << ", ";
        ss << vec[i];
    }
    return ss.str();
}

// 生成路径序列（修正起点逻辑）
std::vector<int> generatePathSequence(int start, int end, std::vector<std::pair<int, int>> &point_mapping, bool is_first_segment)
{
    std::vector<int> sequence;
    if (start == end)
    {
        sequence.push_back(start);
        point_mapping.emplace_back(start, end);
        return sequence;
    }

    // 确定起始点（非首段路径需要调整起点）
    int actual_start = start;
    if (!is_first_segment)
    {
        if (end > start)
        {
            actual_start = start + 1; // 向前路径，起点+1
        }
        else
        {
            actual_start = start - 1; // 向后路径，起点-1
        }
    }

    if (end > actual_start)
    {
        // 向前移动
        for (int i = actual_start; i <= end; ++i)
        {
            sequence.push_back(i);
            point_mapping.emplace_back(i, (i == end) ? end : -1);
        }
    }
    else
    {
        // 向后移动
        for (int i = actual_start; i >= end; --i)
        {
            sequence.push_back(i);
            point_mapping.emplace_back(i, (i == end) ? end : -1);
        }
    }
    return sequence;
}

// 获取表名（根据方向）
std::string getTableName(bool is_forward)
{
    return is_forward ? "car" : "carB";
}

// 获取描述信息
std::string getDescription(const std::string &line_id, bool is_forward)
{
    return line_id + "路线(主表" + (is_forward ? "car)" : "carB)");
}

// 执行单段路径查询
std::vector<car> executeSegmentQuery(
    int map_id, const std::string &line_id,
    const std::vector<int> &segment_sequence,
    size_t segment_index, // 当前是第几段
    int target_point,     // 本段终点
    const std::string &table_name,
    const std::string &description,
    SQLiteWrapper &db)
{

    // 构建SQL查询
    std::string sql = "WITH points(id, num) AS (";
    for (size_t i = 0; i < segment_sequence.size(); ++i)
    {
        if (i != 0)
            sql += " UNION ALL ";
        sql += "SELECT " + std::to_string(i) + ", " + std::to_string(segment_sequence[i]);
    }
    sql += ") SELECT c.x, c.y, c.z, c.lidar_heading, c.ndt_matching,c.rtk_x, c.rtk_y, c.rtk_z, c.rtk_heading, "
           "c.lng, c.lat, c.position_flag, c.orientation_flag, c.num_point, c.stop_flag, c.stop_time, c.line_id, c.map_id,c.ext "
           "FROM " +
           table_name + " c "
                        "JOIN points p ON c.num_point = p.num "
                        "WHERE c.map_id = " +
           std::to_string(map_id) +
           " AND c.line_id = " + line_id + " "
                                           "ORDER BY p.id";

    auto result = db.select(sql);
    if (result.empty())
    {
        throw std::runtime_error("No data found for map_id=" + std::to_string(map_id) +
                                 ", line_id=" + line_id + ", table=" + table_name);
    }

    // 处理查询结果
    std::vector<car> carDataList;
    for (size_t i = 0; i < result.size(); ++i)
    {
        const auto &row = result[i];
        car data;

        // 填充数据
        data.x = std::stod(row[0]);
        data.y = std::stod(row[1]);
        data.z = std::stod(row[2]);
        data.lidar_heading = std::stod(row[3]);
        data.ndt_matching = std::stod(row[4]);
        data.rtk_x = std::stod(row[5]);
        data.rtk_y = std::stod(row[6]);
        data.rtk_z = std::stod(row[7]);
        data.rtk_heading = std::stod(row[8]);
        data.lng = std::stod(row[9]);
        data.lat = std::stod(row[10]);
        data.position_flag = std::stoi(row[11]);
        data.orientation_flag = std::stoi(row[12]);
        data.num_point = std::stoi(row[13]);
        data.stop_flag = std::stoi(row[14]);
        data.stop_time = std::stoi(row[15]);
        data.line_id = std::stoi(row[16]);
        data.map_id = std::stoi(row[17]);
        data.ext = std::stoi(row[18]);
        // 关键点识别
        data.pid = 0;
        data.stopid = target_point; // 所有点都设置当前段的终点作为stopid
        bool is_key_point = false;

        // 检查是否为当前段的终点
        if (data.num_point == target_point)
        {
            data.pid = segment_index + 1; // 段索引从0开始，pid从1开始
            is_key_point = true;

            std::cout << "调试：找到关键点 - 段索引=" << segment_index
                      << ", 索引=" << i
                      << ", num_point=" << data.num_point
                      << ", pid=" << data.pid
                      << ", stopid=" << data.stopid
                      << ", 表名=" << table_name << std::endl;
        }

        // 设置描述信息
        data.describe = description;
        carDataList.push_back(data);
    }

    return carDataList;
}

// 主函数：从数据库获取车辆数据
CarDataResult CronfetchCarDataFromDB(int map_id, const std::string &line_id,
                                     const std::vector<int> &point_ids,
                                     int current_position)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);

    std::vector<car> primaryData;
    std::vector<car> secondaryData;
    std::vector<PathSegment> segments;
    std::vector<std::pair<int, int>> point_mapping;

    if (point_ids.empty())
    {
        throw std::runtime_error("点位列表不能为空");
    }

    // 调试信息
    std::cout << "\n调试: 路线ID='" << line_id << "', 类型="
              << (isNumericLine(line_id) ? "数字路线" : (isAlphaLine(line_id) ? "字母路线" : "未知类型")) << std::endl;
    std::cout << "调试: 目标点位: " << vectorToString(point_ids) << std::endl;

    // 1. 生成所有路径段（修正起点逻辑）
    int current_pos = current_position;
    for (size_t i = 0; i < point_ids.size(); ++i)
    {
        int target = point_ids[i];
        bool is_forward = (target > current_pos);
        bool is_first_segment = (i == 0);

        // 生成当前段路径（非首段路径会自动调整起点）
        auto segment_sequence = generatePathSequence(current_pos, target, point_mapping, is_first_segment);

        // 记录段信息
        PathSegment segment;
        segment.sequence = segment_sequence;
        segment.start = current_pos;
        segment.end = target;
        segment.is_forward = is_forward;
        segment.key_point_index = primaryData.size() + segment_sequence.size() - 1;
        segment.target_stopid = target;
        segments.push_back(segment);

        std::cout << "调试: 第" << i + 1 << "段路径: " << vectorToString(segment_sequence)
                  << ", 方向: " << (is_forward ? "向前" : "向后")
                  << ", 使用表: " << getTableName(is_forward) << std::endl;

        current_pos = target;
    }

    // 2. 按段查询数据
    for (size_t i = 0; i < segments.size(); ++i)
    {
        const auto &seg = segments[i];
        std::string table_name = getTableName(seg.is_forward);
        std::string description = getDescription(line_id, seg.is_forward);

        // 查询当前段数据
        auto segment_data = executeSegmentQuery(
            map_id, line_id, seg.sequence, i, seg.end,
            table_name, description, db);

        // 添加到主数据
        primaryData.insert(primaryData.end(), segment_data.begin(), segment_data.end());

        // 查询辅助表数据
        std::string secondary_table = getTableName(!seg.is_forward);
        std::string secondary_desc = getDescription(line_id, !seg.is_forward);
        auto secondary_segment_data = executeSegmentQuery(
            map_id, line_id, seg.sequence, i, seg.end,
            secondary_table, secondary_desc, db);

        secondaryData.insert(secondaryData.end(), secondary_segment_data.begin(), secondary_segment_data.end());
    }

    return {primaryData, secondaryData};
}

// 获取调度路线B
std::vector<car> BfetchCarDataFromDB(int map_id, int path_id)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    // 查询该路径上的所有点位
    std::string sql =
        "SELECT x, y, z, lidar_heading, ndt_matching, rtk_x, rtk_y, rtk_z, rtk_heading, "
        "lng, lat, position_flag, orientation_flag, num_point, stop_flag, stop_time, line_id, map_id,ext "
        "FROM carB "
        "WHERE map_id = " +
        std::to_string(map_id) +
        " AND line_id = " + std::to_string(path_id) +
        " ORDER BY num_point";

    auto result = db.select(sql);

    if (result.empty())
    {
        ROS_WARN("No data found for map_id=%d, path_id=%d", map_id, path_id);
        return {}; // 返回空vector，且移除throw语句！
    }

    std::vector<car> carDataList;
    for (const auto &row : result)
    {
        car data;
        data.x = std::stod(row[0]);
        data.y = std::stod(row[1]);
        data.z = std::stod(row[2]);
        data.lidar_heading = std::stod(row[3]);
        data.ndt_matching = std::stod(row[4]);
        data.rtk_x = std::stod(row[5]);
        data.rtk_y = std::stod(row[6]);
        data.rtk_z = std::stod(row[7]);
        data.rtk_heading = std::stod(row[8]);
        data.lng = std::stod(row[9]);
        data.lat = std::stod(row[10]);
        data.position_flag = std::stoi(row[11]);
        data.orientation_flag = std::stoi(row[12]);
        data.num_point = std::stoi(row[13]);
        data.stop_flag = std::stoi(row[14]);
        data.stop_time = std::stoi(row[15]);
        data.line_id = std::stoi(row[16]);
        data.map_id = std::stoi(row[17]);
        data.ext = std::stoi(row[18]);
        carDataList.push_back(data);
    }

    return carDataList;
}

// 获取调度路线
std::vector<car> fetchCarDataFromDB(int map_id, int path_id)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    // 查询该路径上的所有点位
    std::string sql =
        "SELECT x, y, z, lidar_heading, ndt_matching, rtk_x, rtk_y, rtk_z, rtk_heading, "
        "lng, lat, position_flag, orientation_flag, num_point, stop_flag, stop_time, line_id, map_id,ext "
        "FROM car "
        "WHERE map_id = " +
        std::to_string(map_id) +
        " AND line_id = " + std::to_string(path_id) +
        " ORDER BY num_point";

    auto result = db.select(sql);

    if (result.empty())
    {
        throw std::runtime_error("No data found for map_id=" + std::to_string(map_id) +
                                 ", path_id=" + std::to_string(path_id));
    }

    std::vector<car> carDataList;
    for (const auto &row : result)
    {
        car data;
        data.x = std::stod(row[0]);
        data.y = std::stod(row[1]);
        data.z = std::stod(row[2]);
        data.lidar_heading = std::stod(row[3]);
        data.ndt_matching = std::stod(row[4]);
        data.rtk_x = std::stod(row[5]);
        data.rtk_y = std::stod(row[6]);
        data.rtk_z = std::stod(row[7]);
        data.rtk_heading = std::stod(row[8]);
        data.lng = std::stod(row[9]);
        data.lat = std::stod(row[10]);
        data.position_flag = std::stoi(row[11]);
        data.orientation_flag = std::stoi(row[12]);
        data.num_point = std::stoi(row[13]);
        data.stop_flag = std::stoi(row[14]);
        data.stop_time = std::stoi(row[15]);
        data.line_id = std::stoi(row[16]);
        data.map_id = std::stoi(row[17]);
        data.ext = std::stoi(row[18]);
        carDataList.push_back(data);
    }

    return carDataList;
}

// 获取记忆路线
std::vector<go_back> fetchgobackDataFromDB()
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    // 查询该路径上的所有点位
    std::string sql =
        "SELECT x, y, z, lidar_heading, rtk_x, rtk_y, rtk_z, rtk_heading, "
        "lng, lat, position_flag, orientation_flag, num_point "
        "FROM go_back ORDER BY num_point DESC";
    auto result = db.select(sql);

    if (result.empty())
    {
        throw std::runtime_error("No data found");
    }

    std::vector<go_back> carDataList;
    carDataList.reserve(result.size()); // 预分配内存

    try
    {
        for (const auto &row : result)
        {
            if (row.size() < 13)
            { // 检查每行数据完整性
                throw std::runtime_error("数据库行数据不完整，预期12列");
            }

            go_back data;
            data.x = std::stod(row[0]);
            data.y = std::stod(row[1]);
            data.z = std::stod(row[2]);
            data.lidar_heading = std::stod(row[3]);
            data.rtk_x = std::stod(row[4]); // 注意：row[4]似乎被跳过了？
            data.rtk_y = std::stod(row[5]);
            data.rtk_z = std::stod(row[6]);
            data.rtk_heading = std::stod(row[7]);
            data.lng = std::stod(row[8]);
            data.lat = std::stod(row[9]);
            data.position_flag = std::stoi(row[10]);
            data.orientation_flag = std::stoi(row[11]);
            data.num_point = std::stoi(row[12]);
            carDataList.push_back(data);
        }
    }
    catch (const std::exception &e)
    {
        throw std::runtime_error(std::string("获取数据失败: ") + e.what());
    }

    return carDataList;
}
// 获取cron总任务包信息
car_cron getcron(int cron_id)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);

    // 查询该任务的信息
    std::string sql =
        "SELECT ts, uid, vehicleCode, executeType, taskCode,turn,uptime "
        "FROM carcron "
        "WHERE id = " +
        std::to_string(cron_id);

    auto result = db.select(sql);

    if (result.empty())
    {
        throw std::runtime_error("No data found for cron_id=" + std::to_string(cron_id));
    }

    // 只处理第一条记录(唯一记录)
    const auto &row = result[0];
    car_cron data;
    data.ts = std::stoi(row[0]);
    data.uid = std::stoi(row[1]);
    data.vehicleCode = row[2];
    data.executeType = std::stoi(row[3]);
    data.taskCode = row[4];
    data.turn = std::stoi(row[5]);
    data.uptime = row[6];
    return data;
}

// 获取地图编号
int getmapid()
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    std::string sql = "SELECT MAX(map_id) FROM fist_pose;";
    auto result = db.select(sql);
    if (!result.empty() && !result[0].empty())
    {
        // 第一行第一列就是 MAX(map_id) 的值
        std::string max_id_str = result[0][0];

        // 如果表为空，max_id_str 可能是空字符串，需要处理
        if (max_id_str.empty())
        {
            return 0; // 或者返回 -1 表示没有记录
        }

        // 转换为整数并返回
        return std::stoi(max_id_str);
    }
    return 0; // 默认返回 0 或 -1 表示失败
}

// 获取任意点调度路线点
std::vector<car> fetchPointdataDataFromDB()
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    // 查询该路径上的所有点位
    std::string sql =
        "SELECT x, y, z, lidar_heading, ndt_matching, rtk_x, rtk_y, rtk_z, rtk_heading, "
        "lng, lat, position_flag, orientation_flag,num_point"
        "FROM point_data "
        " ORDER BY num_point";

    auto result = db.select(sql);

    if (result.empty())
    {
        throw std::runtime_error("No data found");
    }

    std::vector<car> carDataList;
    for (const auto &row : result)
    {
        car data;
        data.x = std::stod(row[0]);
        data.y = std::stod(row[1]);
        data.z = std::stod(row[2]);
        data.lidar_heading = std::stod(row[3]);
        data.ndt_matching = std::stod(row[4]);
        data.rtk_x = std::stod(row[5]);
        data.rtk_y = std::stod(row[6]);
        data.rtk_z = std::stod(row[7]);
        data.rtk_heading = std::stod(row[8]);
        data.lng = std::stod(row[9]);
        data.lat = std::stod(row[10]);
        data.position_flag = std::stoi(row[11]);
        data.orientation_flag = std::stoi(row[12]);
        data.num_point = std::stoi(row[13]);
        data.stop_flag = 0;
        data.stop_time = 0;
        data.line_id = 0;
        data.map_id = 0;
        carDataList.push_back(data);
    }

    return carDataList;
}

// 获取所有基本点
std::vector<point_all> fetchPointallDataFromDB(int map_id)
{
    std::lock_guard<std::mutex> lock(g_db_mutex);
    SQLiteWrapper db(dbName);
    // 查询该路径上的所有点位
    std::string sql =
        "SELECT x, y, z, lidar_heading, ndt_matching, rtk_x, rtk_y, rtk_z, rtk_heading, "
        "lng, lat, position_flag, orientation_flag, map_id,num "
        "FROM point_all "
        "WHERE map_id = " +
        std::to_string(map_id) +
        " ORDER BY num";

    auto result = db.select(sql);

    if (result.empty())
    {
        throw std::runtime_error("No data found for map_id=" + std::to_string(map_id));
    }

    std::vector<point_all> ponit_allDataList;
    for (const auto &row : result)
    {
        point_all data;
        data.x = std::stod(row[0]);
        data.y = std::stod(row[1]);
        data.z = std::stod(row[2]);
        data.lidar_heading = std::stod(row[3]);
        data.ndt_matching = std::stod(row[4]);
        data.rtk_x = std::stod(row[5]);
        data.rtk_y = std::stod(row[6]);
        data.rtk_z = std::stod(row[7]);
        data.rtk_heading = std::stod(row[8]);
        data.lng = std::stod(row[9]);
        data.lat = std::stod(row[10]);
        data.position_flag = std::stoi(row[11]);
        data.orientation_flag = std::stoi(row[12]);
        data.map_id = std::stoi(row[13]);
        data.num = std::stoi(row[14]);
        ponit_allDataList.push_back(data);
    }

    return ponit_allDataList;
}
#endif
